wm = 10000/60*2*pi;  
%电机转速，单位一开始是rpm，所需要除以60再乘以2pi
I_prop = 0.000039337;
%桨叶转动惯量，单位是kg·m^2
I=[0.045 0 0;0 0.045 0;0 0 0.023];
%惯性张量，单位是kg·m^2
L1 = 72.79/1000;
%舵片升力点到轴中心垂直距离，单位m
L2 = 141.67/1000;
%舵片升力中心平面到质心的垂直距离，单位m
m = 2.541;
%飞行器重量，单位kg
T = 0.002;
%离散周期，单位s
n = 10000;
%仿真步数

Ad = [1 T*I_prop*wm/I(1,1) 0;-T*I_prop*wm/I(2,2) 1 0;0 0 1];
Bd = [T*L2/I(1,1) 0 0 0;0 0 T*L2/I(2,2) 0;0 T*L1/I(3,3) 0 -T*L1/I(3,3)];
%状态空间方程矩阵

x = [0.4,0,0.5]';
%状态向量
xd = [0,0,0]';
%期望向量
error = [0,0,0]';
last_error = [0,0,0]';
%误差向量


kp = [5;5;5];
ki = [0;0;0];
kd = [0.2;0.2;0];


u = zeros(4,1);
p = zeros(n,1);
q = zeros(n,1);
r = zeros(n,1);
time = zeros(n,1);
uk = zeros(n,4);
uk(1) = 0;
integral = [0,0,0]';
differential = [0,0,0]';

x = Ad*x + Bd*u; 
%被控对象
error = xd - x;
%误差计算
integral = error;
time(1) = 0;

max_torque = 0.05*0.832;
min_torque = -0.045*0.832;


kf = 0;


for i = 2:n 
   time(i) = (i-1)*T;
   uk(i,:) = u'; 
   p(i) = x(1);
   q(i) = x(2);
   r(i) = x(3);
   x = Ad*x + Bd*u; 
   %被控对象
   last_error = error;
   error = xd - x + kf .* [x(2);-x(1);0];
   %误差计算
   integral = integral + error; 
   %积分项
   differential = (error - last_error)/T;
   %微分项
   u1 = kp(1)*error(1)+ki(1)*integral(1)*T+kd(1)*differential(1);
   u3 = kp(2)*error(2)+ki(2)*integral(2)*T+kd(2)*differential(2);
   u2u4 = kp(3)*error(3)+ki(3)*integral(3)*T+kd(3)*differential(3);
   %控制律
   
   if u1 >= 2*max_torque
       u1 = 2*max_torque;
   elseif u1 <= 2*min_torque
       u1 = 2*min_torque;
   end
   
   if u3 >= 2*max_torque
       u3 = 2*max_torque;
   elseif u3 <= 2*min_torque
       u3 = 2*min_torque;
   end   

   if u2u4 >= 2*max_torque
       u2u4 = 2*max_torque;
   elseif u2u4 <= 2*min_torque
       u2u4 = 2*min_torque;
   end  
   
   u = [u1;u2u4/2;u3;-u2u4/2];
end
figure(1);
plot(time,p,'r',time,q,'b',time,r,'g');
legend('p','q','r');




